#ifndef _SPLINE_H_
#define _SPLINE_H_

#include <limits>
#include <iostream>
#include <cstring>
#include <cmath> 
#include <sstream>
#include <vector>
#include <eigen-3.4.0/Eigen/Dense>

using namespace std;
enum BCT {  
    complete,  //s'(f,a) = f'(a);s'(f,b) = f'(b)
    cubic,  //s''(f,a) = f''(a);s''(f,b) = f''(b)
    natural  //s''(f,a) = 0;s''(f,b) =0
}; 

const double tao = 10 * std::numeric_limits<double>::epsilon();
const double pi = 3.1415926536;



// 函数虚类，实体函数需要继承此类，并定义()运算，子类可以定义diff为导函数，若不定义，则diff默认采用差商代替导数的方法
class Function{
public:
    //函数
    virtual double operator () (const double &x) const = 0;

    //导函数
    virtual double diff (const double &x) const{
        double tmp = ((*this)(x+tao)-(*this)(x-tao)) / (2*tao);

        return tmp;
    }

    //二阶导
    virtual double diff2 (const double &x) const{
        double tmp =((*this)(x+2*tao)+(*this)(x-2*tao)-2*(*this)(x)) / (4*tao*tao);

        return tmp;
    }
};

void error(const std::string & errcode){
    std::cerr << "[Error] " << errcode << std::endl;
    exit(-1);
}

//多项式类
class Poly :public Function{
protected:
public:
    std::vector<double> coef;
    int n;
    double x0=0;

    Poly(){n = 0;}
    /// @brief 
    /// @param coef 
    /// @param x0 
    Poly(const std::vector<double> & coef , const double &x0):
        coef(coef), n(coef.size()-1) ,x0(x0) {}

    Poly(const Poly & p):
        coef(p.coef), n(p.n) , x0(p.x0) {}


    friend std::ostream & operator << (std::ostream & out, const Poly & p){
        bool first = true;
        if(p.coef[0]!=0){
            out << p.coef[0];
            first = false;
        }

        for(int i = 1; i <= p.n; i++){
            if(p.coef[i] != 0 && i !=0){
                if(!first && p.coef[i] >= 0) out << "+";
                if(p.x0> 0){out << p.coef[i] << "(x-"<< p.x0 <<")"<< "^"<<i;}
                if(p.x0 == 0){out << p.coef[i] << "x"<< "^"<<i;}
                if(p.x0 <0){out << p.coef[i] << "(x+"<< -p.x0 <<")"<< "^"<<i;}
                if(first) first = false;
            }
        }
        return out;
    }

    double operator() (const double &x) const{
        double result =coef[n];
        for(int i=0;i<= n-1;i++){
            result = coef[n-i-1]+result*(x-x0);
        }
        return result;
    }
};

 
//PP样条插值
class PPSpline {  
private:
    vector<double> lamda;
    vector<double> mu;
    vector<double> K;
    vector<double> b;


public:  
    int N;  
    vector<Poly> polys;  
    vector<double> x;  
    vector<double> fx;  
    BCT bct;  
    double bct_left;  
    double bct_right; 
  
    double operator() (const double &t) const {  
        for (int i = 0; i < N - 1; i++){  
            if (t >= x[i] && t <= x[i + 1]){  
                return polys[i](t);  
            }  
        } 
    }  
    PPSpline(){};
    //不告知类型为线性插值,S^1_0
    PPSpline(const vector<double>& x, const vector<double>& fx)  
        : x(x), fx(fx){
            N = x.size();
            solution_linear();
        } 

    PPSpline(const vector<double>& x, const vector<double>& fx,  
                   BCT bct, double bct_left, double bct_right)  
        : x(x), fx(fx), bct(bct),  
          bct_left(bct_left), bct_right(bct_right) {
            N = x.size();
            solution_cubic();
        }  

    PPSpline(double lft ,double rht,Function &f, int N, BCT bct)  
        : N(N),bct(bct){
            for(int i=0;i<=N-1;i++){
                x.push_back(lft + i*(rht-lft)/float(N-1));
                fx.push_back(f(lft + i*(rht-lft)/float(N-1)));
            }

            if(bct == complete){
                bct_left = f.diff(lft);
                bct_right = f.diff(rht);
            }
            else if(bct == natural){
                bct_left = 0;
                bct_right = 0;
            }
            else if(bct == cubic){
                bct_left = f.diff2(lft);
                bct_right = f.diff2(rht);
            }

            solution_cubic();
        }  
  
    void solution_linear() {  
        polys.clear();
        polys.resize(N-1);
        for(int i=0;i<N-1;i++){
            vector<double> coefft;
            coefft.push_back(fx[i]);
            coefft.push_back((fx[i+1]-fx[i])/(x[i+1]-x[i]));
            Poly p(coefft,x[i]);
            polys[i] = p;
        }
    }

    void solution_cubic(){
        for(int j=0;j<N-1;j++){
            K.push_back((fx[j+1]-fx[j])/(x[j+1]-x[j]));
        }
        for(int i=0;i<N;i++){
            if(i==0){
                lamda.push_back(1);
                mu.push_back(0);
            }
            else if(i==N-1){
                lamda.push_back(0);
                mu.push_back(1);
            }
            else{
                mu.push_back((x[i]-x[i-1])/(x[i+1]-x[i-1]));
                lamda.push_back((x[i+1]-x[i])/(x[i+1]-x[i-1]));
            }        
        }

        //解矩阵得到二阶导
        Eigen::MatrixXd A(N-2,N-2);
        Eigen::VectorXd bb;
        A = Eigen::MatrixXd::Zero(N-2,N-2); //对矩阵进行初始化。
        bb = Eigen::VectorXd::Zero(N-2);
        if(bct ==complete){
            for(int k=0;k<N-3;k++){
                A(k,k) = 2;
                A(k,k+1) = mu[k+1];
                A(k+1,k) = lamda[k+2];  
            }
            A(N-3,N-3) = 2;

            for(int i =0;i<N-2;i++){
                if(i ==0) bb(i) = 3*mu[1]*K[1]+3*lamda[1]*K[0]-lamda[1]*bct_left;
                else if(i==N-3) bb[i] = 3*mu[N-2]*K[N-2]+3*lamda[N-2]*K[N-3]-mu[N-2]*bct_right;
                else bb[i]=3*mu[i+1]*K[i+1]+3*lamda[i+1]*K[i];
            }
        }

        else if(bct ==cubic || bct ==natural){
            for(int k=0;k<=N-4;k++){
                A(k,k) = 2;
                A(k,k+1) = lamda[k+1];
                A(k+1,k) = mu[k+2];  
            }
            A(N-3,N-3) = 2;

            for(int i =0;i<=N-3;i++){
                if(i ==0) bb[i] = 6*(K[1]-K[0])/(x[2]-x[0])   -mu[1]*bct_left;
                else if(i==N-3) bb[i] = 6*(K[N-2]-K[N-3])/(x[N-1]-x[N-3])-lamda[N-2]*bct_right;
                else bb[i] = 6*(K[i+1]-K[i])/(x[i+2]-x[i]);
            }
        }


        Eigen::VectorXd mm = A.partialPivLu().solve(bb);
        vector<double> m;
        m.push_back(bct_left);
        for(int j=1;j<=N-2;j++){
            m.push_back(mm[j-1]);
        }
        m.push_back(bct_right);

        //计算多项式系数
        for(int i=0;i<N-1;i++){
            polys.resize(N-1);
            vector<double> coefft;
            if(bct ==complete){
                coefft.push_back(fx[i]);
                coefft.push_back(m[i]);
                coefft.push_back((3*K[i]-2*m[i]-m[i+1])/(x[i+1]-x[i]));
                coefft.push_back((m[i]+m[i+1]-2*K[i])/(x[i+1]-x[i])/(x[i+1]-x[i]));
            }

            else if(bct == cubic || bct ==natural){
                coefft.push_back(fx[i]);
                coefft.push_back(K[i]-(m[i+1]+2*m[i])*(x[i+1]-x[i])/6.0  );
                coefft.push_back(m[i]/2.0);
                coefft.push_back((m[i+1]-m[i])/(6.0*(x[i+1]-x[i])));
            }
            Poly p(coefft,x[i]);
            polys[i] = p;
        }                                                                                                                                      
        
    }

};  
  
//B样条基类
class BSpline_base{
protected:
    vector<double> coef;
    vector<double> knots;
    //这里的i范围是1,2,3,...n+N-1
    //k代表k次B样条
    //整个定义域的范围是[t0,t_(N-1)]
    virtual double B(const int &i, const int &k, const double &x) const{
        return k==0 ? (knots[i-1]<x && x<=knots[i]) : 
            (x-knots[i-1])/(knots[i+k-1]-knots[i-1])*B(i,k-1,x) + (knots[i+k]-x)/(knots[i+k]-knots[i])*B(i+1,k-1,x);
    }
    virtual double dB(const int &i, const int &k, const double &x) const{
        return k*B(i, k-1, x) / (knots[i+k-1] - knots[i-1]) - k*B(i+1, k-1, x) / (knots[i+k] - knots[i]);
    }

public:
    BSpline_base(){};

    BSpline_base(const BSpline_base & rhs):
        coef(rhs.coef), knots(rhs.knots) {};


};


//thm 3.58 cardinal 2次样条插值
//传入的是t0,...t_(N-1)为整数
//插值点为t0,t0+1/2,t1+1/2,......t_(N-2)+1/2,t_(N-1),共N+1个 
class BSpline_2 :public BSpline_base{
// private:
//     using BSpline_base::B;
//     using BSpline_base::dB;
    
//     double B(const int &i, const double &x) const{
//         return B(i, 2, x);
//     }
//     double dB(const int &i, const double &x) const{
//         return dB(i, 2, x);
//     }

public:
    BSpline_2(){};
    BSpline_2(const vector<double> &t, const vector<double> &ft){
        int N = t.size();

        for(int i = 0; i < t.size()-1; i++)
            if(t[i] >= t[i+1]) error("BSpline_quadratic :: the knots must be strictly increasing!");
        
        knots.clear();
        knots.push_back(t.front()-2.0);
        knots.push_back(t.front()-1.0);
        for(auto & x : t) knots.push_back(x);
        knots.push_back(t.back()+1.0);
        knots.push_back(t.back()+2.0);

        if(ft.size()!= N+1)
            error("BSpline_quadradic :: Wrong size of vector<double> ft!");

        Eigen::MatrixXd A=Eigen::MatrixXd::Zero(N+1,N+1);
        Eigen::VectorXd b=Eigen::VectorXd::Zero(N+1);
        A(0,0) = B(1,2,t.front());
        A(0,1) = B(2,2,t.front());
        b(0) = ft[0];
        for(int i=1;i<=N-1;i++){
            A(i,i-1)= B(i,2,(t[i-1]+t[i])/2.0);
            A(i,i) = B(i+1,2,(t[i-1]+t[i])/2.0);
            A(i,i+1) = B(i+2,2,(t[i-1]+t[i])/2.0);
            b(i) = ft[i];
        }
        A(N,N-1)=B(N,2,t.back());
        A(N,N) = B(N+1, 2,t.back());
        b(N) = ft[N];

        Eigen::VectorXd m=Eigen::VectorXd::Zero(N+1);
        m = A.partialPivLu().solve(b);
        coef.clear();
        for(int j=0;j<N+1;j++){
            coef.push_back(m[j]);
        }
    }

    BSpline_2(Function &f ,int lft ,int rht){
        vector<double> t;
        vector<double> ft;
        double x = lft;
        t.push_back(x);
        ft.push_back(f(x));
        x = x+0.5;
        while(x<= rht){
            t.push_back(x+0.5);
            ft.push_back(f(x));
            x+=1;
        }
        ft.push_back(f(x-0.5));
        (*this) = BSpline_2(t,ft);
    }

        double operator () (const double &x) const{
        double ans = 0.0;
        for(int i = 0; i <coef.size(); i++)
            ans += coef[i] * B(i+1,2, x);
        return ans;
    };

};

//B3插值
class BSpline_3 :public BSpline_base{
private:
    using BSpline_base::B;
    using BSpline_base::dB;
    
    double B(const int &i, const double &x) const{
        return B(i, 3, x);
    }
    double dB(const int &i, const double &x) const{
        return dB(i, 3, x);
    }

    BCT bct;
    double bct_left;  
    double bct_right; 

public:
    BSpline_3(){};
    BSpline_3(const vector<double> &t, const vector<double> &ft, 
        BCT bct,double bct_left,double bct_right):
        bct(bct),bct_left(bct_left),bct_right(bct_right){
        
        for(int i = 0; i < t.size()-1; i++)
            if(t[i] >= t[i+1]) error("knots should increase!");
        
        knots.clear();
        knots.push_back(t.front()-3.0);
        knots.push_back(t.front()-2.0);
        knots.push_back(t.front()-1.0);
        for(auto & x : t) knots.push_back(x);
        knots.push_back(t.back()+1.0);
        knots.push_back(t.back()+2.0);
        knots.push_back(t.back()+3.0);

        if(ft.size() != t.size())
            error("dim is wrong");

        int N = t.size();
        Eigen::MatrixXd A= Eigen::MatrixXd::Zero(N+2,N+2);
        Eigen::VectorXd b = Eigen::VectorXd::Zero(N+2);
        for(int i=0;i<N+1;i++){
            if(i==0){
                A(0,0) = dB(1,3,t.front());
                A(0,1) = dB(2,3,t.front());
                A(0,2) = dB(3,3,t.front());
                b[0] = bct_left;
            }
            else {
                A(i,i-1) = B(i,3,t[i-1]); 
                A(i,i) = B(i+1,3,t[i-1]); 
                A(i,i+1) = B(i+2,3,t[i-1]); 
                b[i] = ft[i-1];
            }
        }
        A(N+1,N-1) = dB(N,3,t.back());
        A(N+1,N) = dB(N+1,3,t.back());
        A(N+1,N+1) = dB(N+2,3,t.back());
        b[N+1] = bct_right;

        Eigen::VectorXd m = Eigen::VectorXd::Zero(N+2);
        m = A.partialPivLu().solve(b);
        for(int j=0;j<N+2;j++){
            coef.push_back(m[j]);
        }
    }

    BSpline_3(Function &f, const vector<double> &t,BCT bct){
        if(bct == complete){
            vector<double> ft;
            for(auto &x :t){
                ft.push_back(f(x));
            }
            double lft = f.diff(t.front());
            double rht = f.diff(t.back());
            (*this) = BSpline_3(t,ft,bct,lft,rht);
        }
    }

        double operator () (const double &x) const{
        double ans = 0.0;
        for(int i = 0; i <coef.size(); i++)
            ans += coef[i] * B(i+1,3, x);
        return ans;
    };
};


//dim维曲线拟合
template <int dim>
class Curve{
public:
    vector<PPSpline> ppspline_fit;
    // PPSpline Bxs;
    // PPSpline Bys;
    BCT bct = complete;
    double Curve_S;

    // 计算两点在n维空间中的距离  
    double distance(const std::vector<double>& point1, const std::vector<double>& point2) {  
        if (point1.size() != point2.size()) {  
            // 处理维度不匹配的情况  
            return -1;  
        }  
    
        double sum = 0.0;  
        for (size_t i = 0; i < point1.size(); ++i) {  
            double diff = point1[i] - point2[i];  
            sum += diff * diff;  
        }  
        return std::sqrt(sum);  
    } 
    

    Curve(const vector<vector<double>> &knots,const vector<vector<double>> &bcts,BCT bct)
        :bct(bct){
        if(knots.size() != dim){
            error("dim is wrong");
        }

        vector<double >s;
        vector<double> tmp2;
        double d=0;
        for(int i=0;i<knots[0].size();i++){
            vector<double> tmp1;
            for(int j=0;j<knots.size();++j){
                tmp1.push_back(knots[j][i]);
            }  
            if(i==0){
                tmp2=tmp1;
            }
            d+=distance(tmp1,tmp2);
            s.push_back(d);
            tmp2 = tmp1;
        }
        Curve_S = s.back();
        for(int i=0;i<knots.size();i++){
            if(s.size()!=knots[i].size()){
                error("dim is wrong");
            }
            ppspline_fit.push_back(PPSpline(s,knots[i],bct,bcts[i][0],bcts[i][1]));
        }
    }
};



#endif